%% 三维管道生成过程绘画
clear all;
%% path points
waypts=[-5 50 50 20 30;
    50 50 40 40 10;
    10 10 10 10 10];
%% min snap plan generator curve
% set init param
v0 = [0,0,0,0];
a0 = [0,0,0,0];
v1 = [0,0,0,0];
a1 = [0,0,0,0];
total_time=100;
n_order=6;
% time distribution
ts = arrangeT(waypts,total_time);
% generator curve planning
polys_x = minimum_snap_single_axis_simple(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),2);
polys_y = minimum_snap_single_axis_simple(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),2);
polys_z = minimum_snap_single_axis_simple(waypts(3,:),ts,n_order,v0(3),a0(3),v1(3),a1(3),2);
%% draw generator curve
fx=[];fx1=[];fx2=[];fy=[];fy1=[];fy2=[];fz=[];fz1=[];fz2=[];
for i=1:size(polys_x,2)
    tt = ts(i):0.5:ts(i+1);
    xx = polys_vals(polys_x,ts,tt,0);
    xx1 =polys_vals(polys_x,ts,tt,1);
    xx2 =polys_vals(polys_x,ts,tt,2);
    yy = polys_vals(polys_y,ts,tt,0);
    yy1 = polys_vals(polys_y,ts,tt,1);
    yy2 = polys_vals(polys_y,ts,tt,2);
    zz = polys_vals(polys_z,ts,tt,0);
    zz1 = polys_vals(polys_z,ts,tt,1);
    zz2 = polys_vals(polys_z,ts,tt,2); 
    fx=[fx xx];fx1=[fx1 xx1];fx2=[fx2 xx2];
    fy=[fy yy];fy1=[fy1 yy1];fy2=[fy2 yy2];
    fz=[fz zz];fz1=[fz1 zz1];fz2=[fz2 zz2];
end
% figure(2),
% % plot3(fx(1,:),fy(1,:),fz(1,:),'-r','linewidth',2);
% axis equal;
% hold on;
f = [fx;fy;fz];
%% tube surface
% normal vector
ff1 = [fx1;fy1;fz1];
t=ff1./sum(ff1.^2,1).^0.5;
n=diff(t,1,2);
n=[n n(:,end)];
n = n./sum(n.^2,1).^0.5;
% abnormal vector
b = cross(t,n);
% radius
radius = 8;

for k = 1:length(n(1,:))-1
    if n(:,k)'*n(:,k+1)<0
        zero_index = k;
       break; 
    end
end
%% draw cross section
for k=3:zero_index-1
    fo_c=[];fo_xc=[];fo_yc=[];fo_zc=[];
    for theta = 0:0.1:2*pi+0.1
        for k1 = 1:11
            radius_c = 8+0.8-0.8*k1;
            fo_c(:,k1) = f(:,k) + radius_c*(n(:,k)*cos(theta) + b(:,k)*sin(theta));
        end
        %     plot3(fo(1,:),fo(2,:),fo(3,:),'-b');
        fo_xc=[fo_xc; fo_c(1,:)];
        fo_yc=[fo_yc; fo_c(2,:)];
        fo_zc=[fo_zc; fo_c(3,:)];
    end
    figure(2),
    plot3(fx(1,1:zero_index),fy(1,1:zero_index),fz(1,1:zero_index),'-r','linewidth',2);
    hold on;
    fs=surf(fo_xc,fo_yc,fo_zc);
    % fs.EdgeColor =[0.3 0.3 0.3];
    fs.EdgeAlpha = 0;
    fs.FaceAlpha = 0.7;
    fs.FaceColor = [0 0.5 1];
    axis equal;
    grid off;
    axis off;
    fo_x = [];fo_y=[];fo_z=[];
    for theta = 0:0.1:2*pi+0.1 
        radius = 8;
        fo(:,2:k) = f(:,2:k) + radius*(n(:,2:k)*cos(theta) + b(:,2:k)*sin(theta));
    
        fo_x=[fo_x; fo(1,2:k)];
        fo_y=[fo_y; fo(2,2:k)];
        fo_z=[fo_z; fo(3,2:k)];
    end
    fs=surf(fo_x,fo_y,fo_z);
    % fs.EdgeColor =[0.3 0.3 0.3];
    fs.EdgeAlpha = 0;
    fs.FaceAlpha = 0.5;
    fs.FaceColor = [0 0.8 1];
    axis equal;
    axis off;
    grid off;
    
    
    hold off;
end
